home *** CD-ROM | disk | FTP | other *** search
/ System Booster / System Booster.iso / Archives / GNU / GNUPLOTsrc.lha / matrix.c < prev    next >
C/C++ Source or Header  |  1996-01-22  |  8KB  |  376 lines
  1. #ifndef lint
  2. static char *RCSid = "$Id: matrix.c,v 1.10 1994/09/13 16:13:49 alex Exp $";
  3. #endif
  4.  
  5. /*
  6.  *    Matrix algebra, part of
  7.  *
  8.  *    Nonlinear least squares fit according to the
  9.  *    Marquardt-Levenberg-algorithm
  10.  *
  11.  *    added as Patch to Gnuplot (v3.2 and higher)
  12.  *    by Carsten Grammes
  13.  *    Experimental Physics, University of Saarbruecken, Germany
  14.  *
  15.  *    Internet address: cagr@rz.uni-sb.de
  16.  *
  17.  *    Copyright of this module:   Carsten Grammes, 1993
  18.  *
  19.  *    Permission to use, copy, and distribute this software and its
  20.  *    documentation for any purpose with or without fee is hereby granted,
  21.  *    provided that the above copyright notice appear in all copies and
  22.  *    that both that copyright notice and this permission notice appear
  23.  *    in supporting documentation.
  24.  *
  25.  *    This software is provided "as is" without express or implied warranty.
  26.  */
  27.  
  28.  
  29. #define MATRIX_MAIN
  30.  
  31. #include <math.h>
  32.  
  33. #include "type.h"
  34. #include "fit.h"
  35. #include "matrix.h"
  36. #include "stdfn.h"
  37.  
  38. /*****************************************************************/
  39.  
  40. #define Swap(a,b)   {double temp=(a); (a)=(b); (b)=temp;}
  41. #define WINZIG          1e-30
  42.  
  43.  
  44. /*****************************************************************
  45.     internal prototypes
  46. *****************************************************************/
  47. static void lu_decomp __P((double **a, int n, int *indx, double *d));
  48. static void lu_backsubst __P((double **a, int n, int *indx, double b[]));
  49.  
  50.  
  51. /*****************************************************************
  52.     first straightforward vector and matrix allocation functions
  53. *****************************************************************/
  54. double *vec (n)
  55. int n;
  56. {
  57.     /* allocates a double vector with n elements */
  58.     double *dp;
  59.     if( n < 1 )
  60.     return (double *) NULL;
  61.     if ( (dp = (double *) malloc ( n * sizeof(double) )) == NULL )
  62.     Eex ("Memory running low during fit")
  63.     return dp;
  64. }
  65.  
  66.  
  67. int *ivec (n)
  68. int n;
  69. {
  70.     /* allocates a int vector with n elements */
  71.     int *ip;
  72.     if( n < 1 )
  73.     return (int *) NULL;
  74.     if ( (ip = (int *) malloc ( n * sizeof(int) )) == NULL )
  75.     Eex ("Memory running low during fit")
  76.     return ip;
  77. }
  78.  
  79. double **matr (rows, cols)
  80. int rows;
  81. int cols;
  82. {
  83.     /* allocates a double matrix */
  84.  
  85.     register int i;
  86.     register double **m;
  87.  
  88.     if ( rows < 1  ||  cols < 1 )
  89.         return NULL;
  90.     /* allocate pointers to rows */
  91.     if ( (m = (double **) malloc ( rows * sizeof(double *) )) == NULL )
  92.     Eex ("Memory running low during fit")
  93.     /* allocate rows and set pointers to them */
  94.     for ( i=0 ; i<rows ; i++ )
  95.     if ( (m[i] = (double *) malloc (cols * sizeof(double))) == NULL )
  96.         Eex ("Memory running low during fit")
  97.     return m;
  98. }
  99.  
  100.  
  101. void free_matr (m, rows)
  102. double **m;
  103. int rows;
  104. {
  105.     register int i;
  106.     for ( i=0 ; i<rows ; i++ )
  107.     free ( m[i] );
  108.     free (m);
  109. }
  110.  
  111.  
  112. void redim_vec (v, n)
  113. double **v;
  114. int n;
  115. {
  116.     if ( n < 1 ) {
  117.     *v = NULL;
  118.     return;
  119.     }
  120.     if ( (*v = (double *) ralloc (*v, n * sizeof(double), NULL))== NULL )
  121.     Eex ("Memory running low during fit")
  122. }
  123.  
  124. void redim_ivec (v, n)
  125. int **v;
  126. int n;
  127. {
  128.     if ( n < 1 ) {
  129.     *v = NULL;
  130.     return;
  131.     }
  132.     if ( (*v = (int *) ralloc (*v, n * sizeof(int), NULL)) == NULL )
  133.     Eex ("Memory running low during fit")
  134. }
  135.  
  136.  
  137.  
  138. /*****************************************************************
  139.     Linear equation solution by Gauss-Jordan elimination
  140. *****************************************************************/
  141. void solve (a, n, b, m)
  142. double **a;
  143. int n;
  144. double **b;
  145. int m;
  146. {
  147.     int     *c_ix, *r_ix, *pivot_ix, *ipj, *ipk,
  148.         i, ic, ir, j, k, l, s;
  149.  
  150.     double  large, dummy, tmp, recpiv,
  151.         **ar, **rp,
  152.         *ac, *cp, *aic, *bic;
  153.  
  154.     c_ix    = ivec (n);
  155.     r_ix    = ivec (n);
  156.     pivot_ix    = ivec (n);
  157.     memset (pivot_ix, 0, n*sizeof(int));
  158.  
  159.     for ( i=0 ; i<n ; i++ ) {
  160.     large = 0.0;
  161.     ipj = pivot_ix;
  162.     ar = a;
  163.     for ( j=0  ;  j<n  ;  j++, ipj++, ar++ )
  164.         if (*ipj != 1) {
  165.         ipk = pivot_ix;
  166.         ac = *ar;
  167.         for ( k=0  ;  k<n  ;  k++, ipk++, ac++ )
  168.             if ( *ipk ) {
  169.             if ( *ipk > 1 )
  170.                 Eex ("Singular matrix")
  171.             }
  172.             else {
  173.             if ( (tmp = fabs(*ac)) >= large ) {
  174.                 large = tmp;
  175.                 ir = j;
  176.                 ic = k;
  177.             }
  178.             }
  179.         }
  180.     ++(pivot_ix[ic]);
  181.  
  182.     if ( ic != ir ) {
  183.         ac = b[ir];
  184.         cp = b[ic];
  185.         for ( l=0  ;  l<m  ;  l++, ac++, cp++ )
  186.         Swap(*ac, *cp)
  187.         ac = a[ir];
  188.             cp = a[ic];
  189.             for ( l=0  ;  l<n  ;  l++, ac++, cp++ )
  190.                 Swap(*ac, *cp)
  191.         }
  192.  
  193.     c_ix[i] = ic;
  194.         r_ix[i] = ir;
  195.     if ( *(cp = &(a[ic][ic])) == 0.0 )
  196.         Eex ("Singular matrix")
  197.     recpiv = 1/(*cp);
  198.     *cp = 1;
  199.  
  200.     cp = b[ic];
  201.         for ( l=0 ; l<m ; l++ )
  202.             *cp++ *= recpiv;
  203.         cp = a[ic];
  204.     for ( l=0 ; l<n ; l++ )
  205.         *cp++ *= recpiv;
  206.  
  207.     ar = a;
  208.     rp = b;
  209.     for ( s=0 ; s<n ; s++, ar++, rp++ )
  210.         if ( s != ic ) {
  211.         dummy = (*ar)[ic];
  212.         (*ar)[ic] = 0;
  213.         ac = *ar;
  214.         aic = a[ic];
  215.         for ( l=0 ; l<n ; l++ )
  216.             *ac++ -= *aic++ * dummy;
  217.         cp = *rp;
  218.         bic = b[ic];
  219.         for ( l=0 ; l<m ; l++ )
  220.             *cp++ -= *bic++ * dummy;
  221.         }
  222.     }
  223.  
  224.     for ( l=n-1 ; l>=0 ; l-- )
  225.     if ( r_ix[l] != c_ix[l] )
  226.         for ( ar=a, k=0  ;    k<n  ;    k++, ar++ )
  227.         Swap ((*ar)[r_ix[l]], (*ar)[c_ix[l]])
  228.  
  229.     free (pivot_ix);
  230.     free (r_ix);
  231.     free (c_ix);
  232. }
  233.  
  234.  
  235. /*****************************************************************
  236.     LU-Decomposition of a quadratic matrix
  237. *****************************************************************/
  238. static void lu_decomp (a, n, indx, d)
  239. double **a;
  240. int n;
  241. int *indx;
  242. double *d;
  243. {
  244.     int     i, imax, j, k;
  245.  
  246.     double  large, dummy, temp,
  247.         **ar, **lim,
  248.         *limc, *ac, *dp, *vscal;
  249.  
  250.     dp = vscal = vec (n);
  251.     *d = 1.0;
  252.     for ( ar=a, lim = &(a[n]) ; ar<lim ; ar++ ) {
  253.     large = 0.0;
  254.     for ( ac = *ar, limc = &(ac[n]) ; ac<limc ; )
  255.         if ( (temp = fabs (*ac++)) > large )
  256.         large = temp;
  257.     if ( large == 0.0 )
  258.         Eex ("Singular matrix in LU-DECOMP")
  259.     *dp++ = 1/large;
  260.     }
  261.     ar = a;
  262.     for ( j=0 ; j<n ; j++, ar++ ) {
  263.     for ( i=0 ; i<j ; i++ ) {
  264.         ac = &(a[i][j]);
  265.         for ( k=0 ; k<i ; k++ )
  266.         *ac -= a[i][k] * a[k][j];
  267.     }
  268.     large = 0.0;
  269.     dp = &(vscal[j]);
  270.     for ( i=j ; i<n ; i++ ) {
  271.         ac = &(a[i][j]);
  272.             for ( k=0 ; k<j ; k++ )
  273.         *ac -= a[i][k] * a[k][j];
  274.         if ( (dummy = *dp++ * fabs(*ac)) >= large ) {
  275.         large = dummy;
  276.         imax = i;
  277.         }
  278.     }
  279.     if ( j != imax ) {
  280.         ac = a[imax];
  281.         dp = *ar;
  282.         for ( k=0 ; k<n ; k++, ac++, dp++ )
  283.         Swap (*ac, *dp);
  284.         *d = -(*d);
  285.         vscal[imax] = vscal[j];
  286.     }
  287.     indx[j] = imax;
  288.     if ( *(dp = &(*ar)[j]) == 0 )
  289.         *dp = WINZIG;
  290.  
  291.     if ( j != n-1 ) {
  292.         dummy = 1/(*ar)[j];
  293.         for ( i=j+1 ; i<n ; i++ )
  294.         a[i][j] *= dummy;
  295.     }
  296.     }
  297.     free (vscal);
  298. }
  299.  
  300.  
  301. /*****************************************************************
  302.     Routine for backsubstitution
  303. *****************************************************************/
  304. static void lu_backsubst (a, n, indx, b)
  305. double **a;
  306. int n;
  307. int *indx;
  308. double b[];
  309. {
  310.     int     i, memi = -1, ip, j;
  311.  
  312.     double  sum, *bp, *bip, **ar, *ac;
  313.  
  314.     ar = a;
  315.     for ( i=0 ; i<n ; i++, ar++ ) {
  316.     ip = indx[i];
  317.     sum = b[ip];
  318.     b[ip] = b[i];
  319.     if (memi >= 0) {
  320.         ac = &((*ar)[memi]);
  321.         bp = &(b[memi]);
  322.         for ( j=memi ; j<=i-1 ; j++ )
  323.         sum -= *ac++ * *bp++;
  324.     }
  325.         else
  326.         if ( sum )
  327.         memi = i;
  328.     b[i] = sum;
  329.     }
  330.     ar--;
  331.     for ( i=n-1 ; i>=0 ; i-- ) {
  332.     ac = &(*ar)[i+1];
  333.     bp = &(b[i+1]);
  334.     bip = &(b[i]);
  335.     for ( j=i+1 ; j<n ; j++ )
  336.         *bip -= *ac++ * *bp++;
  337.     *bip /= (*ar--)[i];
  338.     }
  339. }
  340.  
  341.  
  342. /*****************************************************************
  343.     matrix inversion
  344. *****************************************************************/
  345. void inverse (src, dst, n)
  346. double **src;
  347. double **dst;
  348. int n;
  349. {
  350.     int i,j;
  351.     int *indx;
  352.     double d, *col, **tmp;
  353.  
  354.     indx = ivec (n);
  355.     col = vec (n);
  356.     tmp = matr (n, n);
  357.     for ( i=0 ; i<n ; i++ )
  358.     memcpy (tmp[i], src[i], n*sizeof(double));
  359.  
  360.     lu_decomp (tmp, n, indx, &d);
  361.  
  362.     for ( j=0 ; j<n ; j++ ) {
  363.     for ( i=0 ; i<n ; i++ )
  364.         col[i] = 0;
  365.     col[j] = 1;
  366.     lu_backsubst (tmp, n, indx, col);
  367.     for ( i=0 ; i<n ; i++ )
  368.         dst[i][j] = col[i];
  369.     }
  370.     free (indx);
  371.     free (col);
  372.     free_matr (tmp, n);
  373. }
  374.  
  375.  
  376.